#include "polar_matrix.h"
#include <cmath>

#define RADIX_LEN 8
#define PI (acos(0.0)*2.0)
#define X_ORIGIN ((double)m.rows())/2
#define Y_ORIGIN ((double)m.cols())/2

/* #define ADJUST(X) pow((X)-4800.0, 5) */
 #define ADJUST(X) X 
void parate() {}
extern int verbose;

void PolarMatrix::toCartesianMatrix(Matrix& m, int rows, int cols) {
	m.setDimension(rows, cols);

	double x_origin= X_ORIGIN;
	double y_origin= Y_ORIGIN;
	for(unsigned i= 0; i<m.rows(); i++) {
		for(unsigned j= 0; j<m.cols(); j++) {
			double x= i - x_origin;
			// ese -1* esta porque si no cuelga horriblemente
			double y= -1*(j - y_origin);
			
			if(verbose) {
				cout << "i= " << i << ", j=" << j << endl;
			}
			if(x==0 && y==0) {
				m(i,j)= ADJUST((*this)(0,0));
				continue;
			}
			if(verbose)
				cout << "x= " << x << ", y= " << y << endl;
			double tita= getAngle(x, y);	
			// para no usar el tita recien calculado
			double r= sqrt(pow(x,2.0) + 
										pow(y,2.0));
			// este radio esta en una escala grande,
			// lo llevo a la escala de esta matriz
			r= r*RADIX_LEN/(sqrt(2)*m.cols());

			m(i,j)= ADJUST(getValue(r, tita)); 
			if(verbose)
				cout << "m(i, j)= " << m(i,j) << endl;
		}
	}
}

double PolarMatrix::getValue(double r, double tita) {
	unsigned radixes[2];
	unsigned angles[2];

	double radix_step= RADIX_LEN/((double)cols()-1);
	radixes[0]= (int)(r/radix_step);
	radixes[1]= (int)(r/radix_step + 1);

	double angle_step= 2*PI/((double)rows());
	angles[0]= (int)(tita/angle_step);
	angles[1]= (int)(tita/angle_step + 1);

	if(angles[1] >= rows()) angles[1]-= rows();
	int nRadixes= radixes[1]>=cols()?1:2;

	if(verbose)
		cout << "r= " << r << ", tita= " << tita << endl;

	double dTot= 0;
	for(int i= 0; i<nRadixes; i++) 
		for(int j= 0; j<2; j++){
			if(verbose)
				cout << "\tr_" << i << "= " << radixes[i]*radix_step 
					<< ", tita_" << j << "= " << angles[j]*angle_step 
					<< ", m(t,r)= " << (*this)(angles[j], radixes[i]) << endl;
			double d=getDist(r,tita, radixes[i]*radix_step, angles[j]*angle_step);
			dTot+= d;
		}

	double res=0;
	for(int i= 0; i<nRadixes; i++) 
		for(int j= 0; j<2; j++) { 
			double d= getDist(r,tita, radixes[i]*radix_step, angles[j]*angle_step);
			res+= (*this)(angles[j], radixes[i])*d;
		}
	
	if(verbose)
		cout << "\tres= " << res/dTot << endl;
	return res/dTot;

}
double PolarMatrix::getDist(double r1, double tita1, 
			double r2, double tita2) {
/* 	double d= sqrt(pow(r1,2) + pow(r2,2) - 2*r1*r2*cos(tita1-tita2)); */
 	double d= sqrt(pow((r1*cos(tita1) + r2*cos(tita2)), 2) +  
 					pow((r1*sin(tita1) + r2*sin(tita2)), 2));  

	return d;
}

double PolarMatrix::getAngle(double x, double y) {
	
	// es un vectorsito apuntando hacia abajo
	if(x==0 && y < 0) return 3*PI/2;
	// es un vectorsito apuntando hacxa arriba
	if(x==0 && y > 0) return PI/2;
	// es un vectorsito apuntando hacxa la izquierda
	if(x<0 && y == 0) return PI;
	// es un vectorsxto apuntando hacxa la derecha
	if(x>0 && y == 0) return 0;
	// primer cuadrante
	if(x>0 && y>0) return atan(y/x);
	// segundo cuadrante
	if(x<0 && y>0) return atan(abs(x/y)) + PI/2;
	// tercer cuadrante
	if(x<0 && y<0) return atan(y/x) + PI;
	// cuarto cuadrante
	if(x>0 && y<0) return atan(abs(x/y)) + 1.5*PI;

	cout << "AAAAA" << endl;
	return -1;
}
ostream& operator<<(ostream& o, PolarMatrix& p) { 
	o << p.rows() << " " << p.cols() - 1 << endl; 

	for(size_t i= 0; i<p.rows(); i++) { 
		for(size_t j= 0; j<p.cols(); j++) {
			o << p(i,j); 
			if(j==p.cols()-1)
				o << endl;
			else
				o << " "; 
		}
	} 

	return o; 
} 
istream& operator>>(istream& is, PolarMatrix& p) { 
	size_t rows, cols;
	is >> rows;
	is >> cols;
	cols++; // para tener en cuenta la columna 0
	p.setDimension(rows, cols);
	
	for(size_t i= 0; i<rows; i++) {
		for(size_t j= 0; j<cols; j++) {
			if(is.eof()) {
				cerr << "Eyy man, me diste menos cosas!\n";
				return is;
			}
			is >> p(i,j);
		}
	}

	return is;
} 
